#include <ros/ros.h>
#include <robot_control/Motion.h>
#include <robot_control/Odometry.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>

#define KEYCODE_R 0x43 
#define KEYCODE_L 0x44
#define KEYCODE_U 0x41
#define KEYCODE_D 0x42
#define KEYCODE_Q 0x71
#define KEYCODE_S 0x73

class TeleopRobot
{
	public:
		TeleopRobot();
		void keyLoop();
		void show_odometry(const robot_control::Odometry::ConstPtr& msg);

	private:

		ros::NodeHandle nh_;
		double linear_, angular_, l_scale_, a_scale_;
		double prev_linear_, prev_angular_;
		ros::Publisher vel_pub_;
		ros::Subscriber odometry_sub_;

};

TeleopRobot::TeleopRobot():
	linear_(0),
	angular_(0),
	l_scale_(5.0),
	a_scale_(2.0)
{
	nh_.param("scale_angular", a_scale_, a_scale_);
	nh_.param("scale_linear", l_scale_, l_scale_);

	vel_pub_ = nh_.advertise<robot_control::Motion>("motion_control", 1);
	odometry_sub_ = nh_.subscribe<robot_control::Odometry>(
			"/robot_odometry", 1000, &TeleopRobot::show_odometry, this);
}

void TeleopRobot::show_odometry(const robot_control::Odometry::ConstPtr& msg)
{
	ROS_DEBUG("Odometry received %ld, %ld", (long int) msg->enc0,
			(long int) msg->enc1);
}

int kfd = 0;
struct termios cooked, raw;

void terminate(int signo) {
	printf("SIGINT receive\n");
	ros::NodeHandle nh;
	ros::Publisher vel_pub;
	vel_pub = nh.advertise<robot_control::Motion>(
			"motion_control", 1);
	robot_control::Motion motion;
	motion.speed = 0;
	motion.rotation = 0;
	motion.reset_priority = 3;
	vel_pub.publish(motion);

	tcsetattr(kfd, TCSANOW, &cooked);

	ros::shutdown();
	exit(0);

}

int main(int argc, char** argv)
{
	void (*val)(int);

	val = signal(SIGINT, terminate);
	ros::init(argc, argv, "teleop_key",
			ros::init_options::NoSigintHandler);

	TeleopRobot teleop_robot;

	ros::AsyncSpinner spinner(2);
	spinner.start();

	teleop_robot.keyLoop();

	return(0);
}


void TeleopRobot::keyLoop()
{
	char c;
	bool dirty=false;
	bool robot_stop = false; 
	robot_control::Motion motion;


	// get the console in raw mode                                                              
	tcgetattr(kfd, &cooked);
	memcpy(&raw, &cooked, sizeof(struct termios));
	raw.c_lflag &=~ (ICANON | ECHO);
	// Setting a new line, then end of file                         
	raw.c_cc[VEOL] = 1;
	raw.c_cc[VEOF] = 2;
	tcsetattr(kfd, TCSANOW, &raw);

	puts("Reading from keyboard");
	puts("---------------------------");
	puts("Use arrow keys to move the robot.");

	prev_linear_=prev_angular_=0;

	while(ros::ok())
	{
		linear_=angular_=0;
		// get the next event from the keyboard  
		if(read(kfd, &c, 1) < 0)
		{
			perror("read():");
			exit(-1);
		}

		ROS_DEBUG("value: 0x%02X\n", c);

		switch(c)
		{
			case KEYCODE_L:
				ROS_DEBUG("LEFT");
				angular_ = 1.0;
				dirty = true;
				break;
			case KEYCODE_R:
				ROS_DEBUG("RIGHT");
				angular_ = -1.0;
				dirty = true;
				break;
			case KEYCODE_U:
				ROS_DEBUG("UP");
				linear_ = 1.0;
				dirty = true;
				break;
			case KEYCODE_D:
				ROS_DEBUG("DOWN");
				linear_ = -1.0;
				dirty = true;
				break;
			case KEYCODE_S:
				ROS_DEBUG("STOP");
				robot_stop = true;
				dirty = true;
				motion.priority = 3;
				break;
		}


		if(dirty == true)
		{
	
			motion.priority = 1;

			if(robot_stop == true)
			{
				motion.speed = 0;
				motion.rotation = 0;
				prev_linear_ = 0;
				prev_angular_ = 0;
			}
			else
			{      
				motion.speed = linear_ * l_scale_ + prev_linear_;
				motion.rotation = angular_ * a_scale_ + prev_angular_;
				prev_linear_ = motion.speed;
				prev_angular_ = motion.rotation;
			}

			dirty=false;
			robot_stop = false;
			ROS_INFO("motin speed %d, rotation %d", (int) motion.speed, (int) motion.rotation);

			vel_pub_.publish(motion);    
		}
	}

	return;
}



